package net.javlov.world;

public class Gyroscope implements Sensor<double[]> {

	protected Body body;
	protected boolean deg;
	
	public Gyroscope() {
		this(false);
	}
	
	public Gyroscope(boolean degrees) {
		deg = degrees;
	}
	
	@Override
	public double[] getReading() {
		double rad = body.getBearing() % (2*Math.PI);
		if ( rad <= -Math.PI )
			rad += 2*Math.PI;
		else if ( rad > Math.PI )
			rad -= 2*Math.PI;
		
		//if ( deg ) {
			long degree = Math.round(rad / (2*Math.PI) * 360);
			if ( degree == -180 )
				degree = 180;
			if ( deg )
				return new double[]{degree};
		//}
		
		return new double[]{degree / 360d};
	}

	@Override
	public int getReadingDim() {
		return 1;
	}

	@Override
	public void init() {}

	@Override
	public void reset() {}

	@Override
	public void setBody(Body b) {
		body = b;
	}

	@Override
	public void setWorld(World w) {}

}
